home *** CD-ROM | disk | FTP | other *** search
/ Disc to the Future 2 / Disc to the Future Part II Programmer's Reference (Wayzata Technology)(6013)(1992).bin / MAC / THINKC / 4_0 / VIVIDUS / VECT.SIT / vect / vect.c < prev    next >
C/C++ Source or Header  |  1991-09-26  |  9KB  |  510 lines
  1. #include    "vect.h"
  2. #include    <math.h>
  3.  
  4. /*    ======================================================================
  5.  
  6.     Vector abstraction library.
  7.     
  8.     This is part of the vect Vividus Source Code Library.  See the
  9.     extern vect.doc documentation file for usage.  See individual
  10.     routines for routine documentation.
  11.     
  12.     Copyright 1991 by Vividus Consulting.
  13.     
  14.     This is not public domain source code.  You may not copy and
  15.     paste from this source code.  Read your Vividus Licensing
  16.     agreement for details and other restrictions.
  17.  
  18.     ======================================================================    */
  19.  
  20. /*    ============================================================    */
  21. /*    Globals:                                --------------------    */
  22.  
  23. vector        vzero    = {0.0,0.0,0.0};
  24. vector        vone    = {1.0,1.0,1.0};
  25.  
  26. static union { short i[6]; double x; } naninit = { 0x3EA1, 0x0331 };
  27.  
  28. //    double number = naninit.x;
  29.  
  30. vector        vnav    = {0.0,0.0,0.0};    //    "Not a vector".
  31.                                         //    Later each component will be
  32.                                         //    replaced by nan(NANINIT).
  33. vector        vxaxis    = {1.0,0.0,0.0};
  34. vector        vyaxis    = {0.0,1.0,0.0};
  35. vector        vzaxis    = {0.0,0.0,1.0};
  36. CoordSys    vWorldCoordSys = {    {1.0, 0.0, 0.0},
  37.                                 {0.0, 1.0, 0.0},
  38.                                 {0.0, 0.0, 1.0}
  39. };
  40.  
  41. /*    ============================================================    */
  42. /*    Vector primitives:                                                */
  43.  
  44. void
  45. vunit (
  46.     vector    *v,
  47.     vector    *vo)
  48. /*
  49.     Find the unit vector of v and return it in vo.
  50. */
  51. {
  52.     vscale (1.0/vmag(v), v, vo);
  53. }
  54.     
  55. void
  56. vcopy (
  57.     vector    *v,
  58.     vector    *vo)
  59. /*
  60.     vo = v
  61. */
  62. {
  63.     vo->x = v->x;
  64.     vo->y = v->y;
  65.     vo->z = v->z;
  66. }
  67.     
  68. void
  69. vscale (
  70.     velem    scalar,
  71.     vector    *v,
  72.     vector    *vo)
  73. /*
  74.     vo = scalar * v
  75. */
  76. {
  77.     vo->x = v->x * scalar;
  78.     vo->y = v->y * scalar;
  79.     vo->z = v->z * scalar;
  80. }
  81.  
  82. void
  83. vsub(
  84.     vector    *a,
  85.     vector    *b,
  86.     vector    *v)
  87. /*
  88.     v = a - b
  89. */
  90. {
  91.     v->x = a->x - b->x;
  92.     v->y = a->y - b->y;
  93.     v->z = a->z - b->z;
  94. }
  95.     
  96. void
  97. vvect (
  98.     vector    *a,
  99.     vector    *b,
  100.     vector    *v)
  101. /*
  102.     Find the vector going from point a to point b. 
  103. */
  104. {
  105.     v->x = b->x - a->x;
  106.     v->y = b->y - a->y;
  107.     v->z = b->z - a->z;
  108. }
  109.     
  110. void
  111. vadd (
  112.     vector    *a,
  113.     vector    *b,
  114.     vector    *v)
  115. /*
  116.     v = a + b
  117. */
  118. {
  119.     v->x = a->x + b->x;
  120.     v->y = a->y + b->y;
  121.     v->z = a->z + b->z;
  122.     return;
  123.     
  124.     /*    The following notice may not be removed under any
  125.         circumstance.  See your licensing agreement.        */
  126.     asm {
  127.         dc.b    "vect Copyright 1991 Vividus Consulting"
  128.     }
  129. }
  130.     
  131. void
  132. vsadd(
  133.     vector    *b,
  134.     velem    m,
  135.     vector    *x,
  136.     vector    *v)
  137. /*
  138.     v = b + mx
  139. */
  140. {
  141.     register velem    vx = b->x + m * x->x,
  142.                     vy = b->y + m * x->y,
  143.                     vz = b->z + m * x->z;
  144.     
  145.     v->x = vx;
  146.     v->y = vy;
  147.     v->z = vz;
  148. }
  149. void
  150. vcross (
  151.     vector    *a,
  152.     vector    *b,
  153.     vector    *v)
  154. /*
  155.     v = a cross b
  156. */
  157. {
  158.     register velem    vx = (a->y * b->z) - (a->z * b->y),
  159.                     vy = (a->z * b->x) - (a->x * b->z),
  160.                     vz = (a->x * b->y) - (a->y * b->x);
  161.  
  162.     v->x = vx;
  163.     v->y = vy;
  164.     v->z = vz;
  165. }
  166.     
  167. velem    vdot (
  168.     vector    *a,
  169.     vector    *b)
  170. /*
  171.     Returns the dot product of a and b.
  172. */
  173. {
  174.     return(a->x * b->x + a->y * b->y + a->z * b->z);
  175. }
  176.     
  177. velem
  178. vmag (
  179.     vector    *v)
  180. /*
  181.     Returns the magnitude of v. 
  182. */
  183. {
  184.     double    retval;
  185.     
  186.     retval = v->x * v->x;
  187.     retval += v->y * v->y;
  188.     retval += v->z * v->z;
  189.     retval = sqrt(retval);
  190.     
  191.     return (retval);
  192. }
  193.  
  194. velem
  195. vdist(vector *a, vector *b)
  196. /*
  197.     Returns the distance between the two "points."
  198. */
  199. {
  200.     vector    v;
  201.     
  202.     vsub(b, a, &v);
  203.     return(vmag(&v));
  204. }
  205. velem
  206. vangle (
  207.     vector    *a,
  208.     vector    *b)
  209. /*
  210.     Gives the angle (radians) between the two vectors.
  211.     
  212.     The direction of the vectors is important.
  213. */
  214. {
  215.     return( acos( vdot(a, b) / (vmag(a) * vmag(b)) ) );
  216. }  
  217.     
  218. void
  219. vmul (
  220.     vector    *a,
  221.     vector    *b,
  222.     vector    *v)
  223. /*
  224.     Multiply the components of vector b by the components of a (and store in v). 
  225. */
  226. {
  227.     register velem    vx = a->x * b->x,
  228.                     vy = a->y * b->y,
  229.                     vz = a->z * b->z;
  230.     
  231.     v->x = vx;
  232.     v->y = vy;
  233.     v->z = vz;
  234. }
  235.     
  236. /*    ============================================================    */
  237. /*    Useful vector routines:                                            */
  238.  
  239. void
  240. vsphere2v (
  241.     double    rho,
  242.     double    phi,
  243.     double    theta,
  244.     vector    *v)
  245. /*
  246.     Given the spherical coordinates, the cartesian point is returned.
  247. */
  248. {
  249.     v->x = rho * sin(phi) * cos(theta);
  250.     v->y = rho * sin(phi) * sin(theta);
  251.     v->z = rho * cos(phi);
  252. }
  253.     
  254. void
  255. vv2sphere (
  256.     vector    *v,
  257.     double    *rho,
  258.     double    *phi,
  259.     double    *theta)
  260. /*
  261.     Given the cartesian point, the spherical coordinates are returned.
  262. */
  263. {
  264.     *rho = vmag(v);
  265.     *theta = atan2(v->y, v->x);
  266.     *phi = vangle(v, &vzaxis);
  267. }
  268.  
  269. void
  270. vnorm(vector x[], vector *norm)
  271. /*
  272.     Returns the unit "normal" of the plane identified by the first three points
  273.     of x.  The "outside" of the plane is determined by the clockwise order of
  274.     the points.
  275. */
  276. {
  277.     vector    a, b;
  278.     
  279.     vvect(&x[0], &x[1], &a);
  280.     vvect(&x[1], &x[2], &b);
  281.     vcross(&a, &b, norm);
  282.     vunit(norm, norm);
  283. }
  284.     
  285. void
  286. vmatmul    (
  287.     vector    *a,
  288.     vector    m[],    /*    A 3x3 matrix.  The first vector is the first row.    */
  289.     vector    *v)
  290. /*
  291.     A vector and 3x3 matrix multiplication routine.
  292.     
  293.     v = a * m
  294. */
  295. {
  296.     vector    *m1,
  297.             *m2,
  298.             *m3;
  299.     register velem vx, vy, vz;
  300.     
  301.     m1 = &(m[0]);
  302.     m2 = &(m[1]);
  303.     m3 = &(m[2]);
  304.     vx =  a->x * m1->x + a->y * m1->y + a->z * m1->z;
  305.     vy =  a->x * m2->x + a->y * m2->y + a->z * m2->z;
  306.     vz =  a->x * m3->x + a->y * m3->y + a->z * m3->z;
  307.     v->x = vx;
  308.     v->y = vy;
  309.     v->z = vz;
  310. }
  311.  
  312. void
  313. vcmin(
  314.     vector    *a,
  315.     vector    *b,
  316.     vector    *out)
  317. /*
  318.     This function returns the component minimums of a & b in out.
  319. */
  320. {
  321.     if (a->x < b->x)
  322.         out->x = a->x;
  323.     else
  324.         out->x = b->x;
  325.  
  326.     if (a->y < b->y)
  327.         out->y = a->y;
  328.     else
  329.         out->y = b->y;
  330.  
  331.     if (a->z < b->z)
  332.         out->z = a->z;
  333.     else
  334.         out->z = b->z;
  335. }
  336.     
  337. void
  338. vcmax(
  339.     vector    *a,
  340.     vector    *b,
  341.     vector    *out)
  342. /*
  343.     This function returns the component maximums of a & b in out.
  344. */
  345. {
  346.     if (a->x > b->x)
  347.         out->x = a->x;
  348.     else
  349.         out->x = b->x;
  350.  
  351.     if (a->y > b->y)
  352.         out->y = a->y;
  353.     else
  354.         out->y = b->y;
  355.  
  356.     if (a->z > b->z)
  357.         out->z = a->z;
  358.     else
  359.         out->z = b->z;
  360. }
  361.  
  362. Boolean vvalid(vector *v)
  363. /*
  364.     Returns true if every component of v is a "valid" floating point number.
  365.     
  366.     Note:    Presently, this just makes sure no component of v is a coponent
  367.             of vnav.
  368. */
  369. {
  370.     if ( (v->x == vnav.x) || (v->y == vnav.y) || (v->z == vnav.z) )
  371.         return (false);
  372.     else
  373.         return (true);
  374. }
  375.  
  376. /*    ============================================================    */
  377. /*    Operations on point/vector lists:                                */
  378.  
  379. void
  380. vCopy(int n, vector x[], vector xn[])
  381. /*
  382.     Copies the point list x to xn.
  383. */
  384. {
  385.     int        i;
  386.     
  387.     for (i = 0; i < n; i++) {
  388.         vcopy(&x[i], &xn[i]);
  389.     }
  390. }
  391.  
  392. void
  393. vTranslate(int n, vector x[], vector *b, vector xn[])
  394. /*
  395.     Translate the point list x by the amount of vector b.
  396.     Results are in xn.
  397. */
  398. {
  399.     int        i;
  400.     
  401.     for (i = 0; i < n; i++) {
  402.         vadd(&x[i], b, &xn[i]);
  403.     }
  404. }
  405.  
  406. void
  407. vScale(int n, vector x[], vector *s, vector xn[])
  408. /*
  409.     Scale each component of the vectors in the vector list by the
  410.     specified amounts in the components of s.
  411.     Results are in xn.
  412. */
  413. {
  414.     int        i;
  415.     
  416.     for (i = 0; i < n; i++) {
  417.         vmul(&x[i], s, &xn[i]);
  418.     }
  419. }
  420.  
  421. void
  422. vRotate3(
  423.     int n, vector x[], 
  424.     vector *a, vector *b, vector *c,
  425.     vector xn[])
  426. /*
  427.     Rotate the point list about the axis defined by the vector
  428.         (b - a) cross (c - b)
  429.     and the location
  430.         b.
  431.     
  432.     The amount and direction of rotation is determined by the angle
  433.     and angle direction between the vectors:
  434.         (b - a), and (c - b).
  435.         
  436.     Results are in xn.
  437. */
  438. {
  439.     vector    av, bv, norm, xa, ya;
  440.     double    angle;
  441.     register double    dy, dx;
  442.     
  443.     vsub(a, b, &av);
  444.     vunit(&av, &xa);
  445.     vsub(c, b, &bv);
  446.     vcross(&av, &bv, &norm);
  447.     vcross(&xa, &norm, &ya);
  448.     vunit(&ya, &ya);
  449.     
  450.     dy = vdot(&ya, &bv);
  451.     dx = vdot(&xa, &bv);
  452.     angle = atan2(dy, dx);
  453.  
  454.     vRotate(n, x, b, &norm, angle, xn);
  455. }
  456.  
  457. void
  458. vRotate(int n, vector x[], vector *o, vector *norm, velem angle, vector xn[])
  459. /*
  460.     Rotate the point list about the given axis by the given angle.
  461.     
  462.     The point o and the direction norm determine the axis of rotation.
  463.     
  464.     Results are in xn.
  465. */
  466. {
  467. /*
  468.     From:
  469.         C Park, "Interactive Microcomputer Graphics," Addison Wesley, 1985, p149.
  470. */
  471.  
  472.     vector    q, *qp = &q, *xi, *xo;
  473.     vector    mat[3];
  474.     velem    ct = cos(angle);
  475.     velem    st = sin(angle);
  476.     int        i;
  477.  
  478.     #define    A    (mat[0].x)    
  479.     #define    B    (mat[0].y)    
  480.     #define    C    (mat[0].z)    
  481.     #define    D    (mat[1].x)    
  482.     #define    E    (mat[1].y)    
  483.     #define    F    (mat[1].z)    
  484.     #define    G    (mat[2].x)    
  485.     #define    H    (mat[2].y)    
  486.     #define    I    (mat[2].z)    
  487.     #define    a    (qp->x)
  488.     #define    b    (qp->y)
  489.     #define    c    (qp->z)
  490.  
  491.     vunit (norm, &q);
  492.     A = a * a + (1.0 - a * a) * ct;
  493.     B = a * b * (1.0 - ct) + c * st;
  494.     C = a * c * (1.0 - ct) - b * st;
  495.     D = a * b * (1.0 - ct) - c * st;
  496.     E = b * b + (1.0 - b * b) * ct;
  497.     F = b * c * (1.0 - ct) + a * st;
  498.     G = a * c * (1.0 - ct) + b * st;
  499.     H = b * c * (1.0 - ct) - a * st;
  500.     I = c * c + (1.0 - c * c) * ct;
  501.     
  502.     for (i = 0; i < n; i++) {
  503.         xi = &x[i];
  504.         xo = &xn[i];
  505.         vsub(xi, o, xo);
  506.         vmatmul(xo, mat, xo);
  507.         vadd(xo, o, xo);
  508.     }
  509. }
  510.